package scu.maqiang.homogenization;

import scu.maqiang.fes.*;
import scu.maqiang.fes.FES2T31;
import scu.maqiang.mesh.MatrixFunc;
import scu.maqiang.mesh.Mesh2T3;
import scu.maqiang.mesh.Mesh2T6;
import scu.maqiang.mesh.ScalarFunc;
import scu.maqiang.numeric.CG;
import scu.maqiang.numeric.Direct;
import scu.maqiang.numeric.DoubleIntegerPair;
import scu.maqiang.numeric.MVO;
import scu.maqiang.numeric.NewIterSSolver;
import scu.maqiang.numeric.Pair;
import scu.maqiang.numeric.SRMatrix;

public class SOTS2DT362 {

	public SOTS2DT362(FES2T32 fsT3, FES2T62 fsT6) {
		this.fsT3 = fsT3;
		this.fsT6 = fsT6;
	}

	public void elasticityCellSolution(MatrixFunc conFunc, double[][][] param, BVPType tp, double[][] FOCS,
			double[][] D, double[][] SOCS) {
		SRMatrix K = new SRMatrix(fsT3.GetNdof());
		fsT3.assembleElasticStiff(conFunc, param, tp, K);
		SRMatrix K1 = new SRMatrix(K);
		double[][] RHS = new double[3][fsT3.GetNdof()];
		fsT3.assembleGlobalVector(conFunc, param, SOTS2DT362::elementElasticityFirstOrder, tp, RHS);
		
		Pair<double[], int[]> dof13Y = fsT3.getDiagElementsFromBoundaries(K, Direct.Y, 1, 3);
		Pair<double[], int[]> dof24X = fsT3.getDiagElementsFromBoundaries(K, Direct.X, 2, 4);
		
		MVO.fill(FOCS, 0.0);
		fsT3.applyBC_MBN(K, Direct.Y, 1, 3);
		fsT3.applyBC_MBN(K, Direct.X, 2, 4);
		NewIterSSolver solver = new NewIterSSolver(K);
		fsT3.applyBC_MBN(RHS[0], Direct.Y, 0.0, 1, 3);
		fsT3.applyBC_MBN(RHS[0], Direct.X, 0.0, 2, 4);
		fsT3.applyBC_MBN(RHS[1], Direct.Y, 0.0, 1, 3);
		fsT3.applyBC_MBN(RHS[1], Direct.X, 0.0, 2, 4);
		solver.PCGSSOR(RHS[0], FOCS[0], 1.75, 1); // 11
		solver.PCGSSOR(RHS[1], FOCS[1], 1.75, 1); // 22
		
		int idx = 0;
		double[] value13Y = dof13Y.getFirst();
		for(int dof : dof13Y.getSecond()) {
			K.setElement(dof, dof, value13Y[idx++]);
		}
		double[] value24X = dof24X.getFirst();
		idx = 0;
		for(int dof : dof24X.getSecond()) {
			K.setElement(dof, dof, value24X[idx++]);
		}

		
		fsT3.applyBC_MBN(K, Direct.Y, 2, 4);
		fsT3.applyBC_MBN(K, Direct.X, 1, 3);
		fsT3.applyBC_MBN(RHS[2], Direct.Y, 0.0, 2, 4);
		fsT3.applyBC_MBN(RHS[2], Direct.X, 0.0, 1, 3);
		solver.setMatrix(K);
		solver.PCGSSOR(RHS[2], FOCS[2], 1.75, 1); // 12

		computeHomogenizedElasticity(conFunc, param, FOCS, D);
		
		RHS = new double[6][fsT3.GetNdof()];
		fsT3.assembleGlobalVector(conFunc, param, FOCS, D, SOTS2DT362::elementElasticitySecondOrder, tp, RHS);
		MVO.fill(SOCS, 0.0);
		
		fsT3.applyBC_MBN(K, Direct.All, 1, 2, 3, 4);
		solver.setMatrix(K);
		for(int i = 0; i < 6; i++) {
			fsT3.applyBC_MBN(RHS[i], Direct.All, 0.0, 1, 2, 3, 4);
			solver.PCGSSOR(RHS[i], SOCS[i], 1.75, 1);
		}
	}
	
	public void elasticityEigenValueCellSolution(MatrixFunc conFunc, ScalarFunc[] rhoFunc, double[][][] param, BVPType tp, double[][] FOCS,
			double[][] D, double[] homoRho, double[][] SOCS) {
		SRMatrix K = new SRMatrix(fsT3.GetNdof());
		fsT3.assembleElasticStiff(conFunc, param, tp, K);
		double[][] RHS = new double[3][fsT3.GetNdof()];
		fsT3.assembleGlobalVector(conFunc, param, SOTS2DT362::elementElasticityFirstOrder, tp, RHS);
		fsT3.applyBC_MBN(K, Direct.All, 1, 2, 3, 4);
		
		MVO.fill(FOCS, 0.0);
		NewIterSSolver solver = new NewIterSSolver(K);
		for(int i = 0; i < 3; i++) {
			fsT3.applyBC_MBN(RHS[i], Direct.All, 0.0, 1, 2, 3, 4);
			solver.PCGSSOR(RHS[i], FOCS[i], 1.75, 1); // 11
		}
		
		computeHomogenizedElasticity(conFunc, param, FOCS, D);
		
		FES2T31 fs1 = new FES2T31((Mesh2T3)(fsT3.mesh));
		double[] iaValue = fs1.integrate(rhoFunc, null);
		for(int i = 0, n = iaValue.length; i < n; i++) {
			homoRho[i] = iaValue[i];
		}

		
		RHS = new double[6][fsT3.GetNdof()];
		double[][] DD = new double[4][];
		DD[0] = D[0];
		DD[1] = D[1];
		DD[2] = D[2];
		DD[3] = homoRho;
		fsT3.assembleGlobalVector(conFunc, rhoFunc, param, FOCS, D, SOTS2DT362::elementElasticityEigenValueSecondOrder, tp, RHS);
		MVO.fill(SOCS, 0.0);
		
		for(int i = 0; i < 6; i++) {
			fsT3.applyBC_MBN(RHS[i], Direct.All, 0.0, 1, 2, 3, 4);
			solver.PCGSSOR(RHS[i], SOCS[i], 1.75, 1);
		}
	}

	public static void elementElasticityFirstOrder(int[] ele, double[][] xy, double[][] coef, BVPType tp, double[][] eleXY) {
		double[][] grad = new double[3][2];
		double area = CG.gradient2DT3(xy, grad);
		double[][] B = new double[6][3];
		B[0][0] = B[1][2] = -grad[0][0] * area;
		B[2][0] = B[3][2] = -grad[1][0] * area;
		B[4][0] = B[5][2] = -grad[2][0] * area;
		B[1][1] = B[0][2] = -grad[0][1] * area;
		B[3][1] = B[2][2] = -grad[1][1] * area;
		B[5][1] = B[4][2] = -grad[2][1] * area;

		eleXY[0] = MVO.matmul(B, coef[0]);
		eleXY[1] = MVO.matmul(B, coef[1]);
		eleXY[2] = MVO.matmul(B, coef[2]);
	}

	public void computeHomogenizedElasticity(MatrixFunc conFunc, double[][][] param, double[][] FOCS, double[][] D) {
		double[][] grad = new double[3][2];
		double[][] xy = new double[3][2];
		Mesh2T3 mesh = (Mesh2T3) fsT3.getMesh();
		double[][] N = new double[3][6];
		double[][] B = new double[3][6];
		double wholeArea = 0.0;
		MVO.fill(D, 0.0);
		for (int k = 0; k < mesh.getNt(); k++) {
			int[] ele = mesh.getElement(k);
			mesh.getCoordPerElement(ele, xy);
			int[] dof = fsT3.getDof(ele);
			MVO.getSlice(FOCS[0], dof, N[0]);
			MVO.getSlice(FOCS[1], dof, N[1]);
			MVO.getSlice(FOCS[2], dof, N[2]);
			double area = CG.gradient2DT3(xy, grad);

			B[0][0] = B[2][1] = grad[0][0];
			B[0][2] = B[2][3] = grad[1][0];
			B[0][4] = B[2][5] = grad[2][0];
			B[1][1] = B[2][0] = grad[0][1];
			B[1][3] = B[2][2] = grad[1][1];
			B[1][5] = B[2][4] = grad[2][1];
			double[] ip = mesh.getinteriorPointInElement(xy);
			double[][] con = conFunc.action(ip, mesh.getElementLabel(k), param);
			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 3; j++) {
					D[i][j] += (con[i][j] + MVO.dot_product(con[i], MVO.matmul(B, N[j]))) * area;
				}
			}
			wholeArea += area;
		}
		MVO.mul(D, 1.0 / wholeArea);
	}

	public static void elementElasticitySecondOrder(int[] ele, double[][] xy, double[][] con, double[][] FOCS, double[][] D,
			BVPType tp, double[][] eleXY) {
		double[][] grad = new double[3][2];
		double area = CG.gradient2DT3(xy, grad);
		double area3 = area / 3.0;
		double[][] B = new double[3][6];
		B[0][0] = B[2][1] = grad[0][0];
		B[0][2] = B[2][3] = grad[1][0];
		B[0][4] = B[2][5] = grad[2][0];
		B[1][1] = B[2][0] = grad[0][1];
		B[1][3] = B[2][2] = grad[1][1];
		B[1][5] = B[2][4] = grad[2][1];
		double[][] BT = MVO.transpose(B);

		double[][] w = { { area3, 0.0 }, { 0.0, area3 }, { area3, 0.0 }, { 0.0, area3 }, { area3, 0.0 },
				{ 0.0, area3 } };
		double[][] wT = MVO.transpose(w);

		double[] C = new double[2];
		double[] part1 = null;
		double[][] con2 = new double[2][];
		double[][] con2T = null;
		double[] part2 = null;


		
		for (int k = 0; k < 3; k++) {
			C[0] = con[0][k] - D[0][k] + MVO.dot_product(con[0], MVO.matmul(B, FOCS[k]));
			C[1] = con[2][k] - D[2][k] + MVO.dot_product(con[2], MVO.matmul(B, FOCS[k]));
			part1 = MVO.matmul(w, C);

			con2[0] = con[0];
			con2[1] = con[2];
			con2T = MVO.transpose(con2);
			part2 = MVO.matmul(MVO.matmul(MVO.matmul(BT, con2T), wT), FOCS[k]);

			for (int i = 0; i < 6; i++) {
				eleXY[2 * k][i] = part1[i] - part2[i]; // 111
			}

			// 121
			C[0] = con[2][k] - D[2][k] + MVO.dot_product(con[2], MVO.matmul(B, FOCS[k]));
			C[1] = con[1][k] - D[1][k] + MVO.dot_product(con[1], MVO.matmul(B, FOCS[k]));
			part1 = MVO.matmul(w, C);

			con2[0] = con[2];
			con2[1] = con[1];
			con2T = MVO.transpose(con2);
			part2 = MVO.matmul(MVO.matmul(MVO.matmul(BT, con2T), wT), FOCS[k]);
			for (int i = 0; i < 6; i++) {
				eleXY[2 * k + 1][i] = part1[i] - part2[i]; // 121
			}
		}
	}

	
	public static void elementElasticityEigenValueSecondOrder(int[] ele, double[][] xy, double[][] con, double[][] rho, double[][] FOCS, double[][] D, BVPType tp,
			double[][] eleXY) {
		double[][] grad = new double[3][2];
		double area = CG.gradient2DT3(xy, grad);
		double area3 = area / 3.0;
		double[][] B = new double[3][6];
		B[0][0] = B[2][1] = grad[0][0];
		B[0][2] = B[2][3] = grad[1][0];
		B[0][4] = B[2][5] = grad[2][0];
		B[1][1] = B[2][0] = grad[0][1];
		B[1][3] = B[2][2] = grad[1][1];
		B[1][5] = B[2][4] = grad[2][1];
		double[][] BT = MVO.transpose(B);

		double[][] w = { { area3, 0.0 }, { 0.0, area3 }, { area3, 0.0 }, { 0.0, area3 }, { area3, 0.0 },
				{ 0.0, area3 } };
		double[][] wT = MVO.transpose(w);

		double[] C = new double[2];
		double[] part1 = null;
		double[][] con2 = new double[2][];
		double[][] con2T = null;
		double[] part2 = null;

		double sumRho = MVO.sum(rho[0]);
		double[] vRho = {0.25 * (sumRho + rho[0][0]), 0.25 * (sumRho + rho[0][1]), 0.25 * (sumRho + rho[0][2])};
		double hRho = D[3][0];
		
		for (int k = 0; k < 3; k++) {
			C[0] = con[0][k] - D[0][k] * vRho[k] / hRho + MVO.dot_product(con[0], MVO.matmul(B, FOCS[k]));
			C[1] = con[2][k] - D[2][k] * vRho[k] / hRho + MVO.dot_product(con[2], MVO.matmul(B, FOCS[k]));
			part1 = MVO.matmul(w, C);

			con2[0] = con[0];
			con2[1] = con[2];
			con2T = MVO.transpose(con2);
			part2 = MVO.matmul(MVO.matmul(MVO.matmul(BT, con2T), wT), FOCS[k]);

			for (int i = 0; i < 6; i++) {
				eleXY[2 * k][i] = part1[i] - part2[i]; // 111
			}

			// 121
			C[0] = con[2][k] - D[2][k] * vRho[k] / hRho + MVO.dot_product(con[2], MVO.matmul(B, FOCS[k]));
			C[1] = con[1][k] - D[1][k] * vRho[k] / hRho + MVO.dot_product(con[1], MVO.matmul(B, FOCS[k]));
			part1 = MVO.matmul(w, C);

			con2[0] = con[2];
			con2[1] = con[1];
			con2T = MVO.transpose(con2);
			part2 = MVO.matmul(MVO.matmul(MVO.matmul(BT, con2T), wT), FOCS[k]);
			for (int i = 0; i < 6; i++) {
				eleXY[2 * k + 1][i] = part1[i] - part2[i]; // 121
			}
		}		
	}
	
	
	public void assembleElasticitySOTSSolution(FES2T32 fsFine, double[][] uv_0, double[][] uv_1, double[][] uv_2,
											   FES2T62 fsHomo, double[][] uv_Homo, FES2T32 fsCell, double[][] FOCS, double[][] SOCS, int N, double ep) {
		double[] xy = new double[2];
		double[] cellXY = new double[2];

		double[] eleVal6 = new double[6];
		double[] eleVal3 = new double[3];

		double[][] grad = new double[4][fsHomo.mesh.getNv()];
		double[][] Se = new double[6][fsHomo.mesh.getNv()];
		// fsHomo.computeGradient(T_Homo, grad);

		fsHomo.computeFirstAndSecondDerivative(uv_Homo, grad, Se);

		double corrector1, corrector2;

		DoubleIntegerPair homoData = new DoubleIntegerPair(0.0, 0);
		int[] eleIdx6 = fsHomo.mesh.getElement(0).clone();

		DoubleIntegerPair cellData = new DoubleIntegerPair(0.0, 0);
		int[] eleIdx3 = fsCell.mesh.getElement(0).clone();
		double val = 0.0;

		FES2T31 fsCC = new FES2T31((Mesh2T3) (fsCell.mesh));
		FES2T61 fsHH = new FES2T61((Mesh2T6) (fsHomo.mesh));

		double[][] FOCS11_uv = new double[2][fsCell.mesh.getNv()];
		double[][] FOCS22_uv = new double[2][fsCell.mesh.getNv()];
		double[][] FOCS12_uv = new double[2][fsCell.mesh.getNv()];
		fsCell.extractComponent(FOCS[0], FOCS11_uv);
		fsCell.extractComponent(FOCS[1], FOCS22_uv);
		fsCell.extractComponent(FOCS[2], FOCS12_uv);

		double[][] SOCS111_uv = new double[2][fsCell.mesh.getNv()];
		double[][] SOCS121_uv = new double[2][fsCell.mesh.getNv()];
		double[][] SOCS212_uv = new double[2][fsCell.mesh.getNv()];
		double[][] SOCS222_uv = new double[2][fsCell.mesh.getNv()];
		double[][] SOCS112_uv = new double[2][fsCell.mesh.getNv()];
		double[][] SOCS122_uv = new double[2][fsCell.mesh.getNv()];

		fsCell.extractComponent(SOCS[0], SOCS111_uv);
		fsCell.extractComponent(SOCS[1], SOCS121_uv);
		fsCell.extractComponent(SOCS[2], SOCS212_uv);
		fsCell.extractComponent(SOCS[3], SOCS222_uv);
		fsCell.extractComponent(SOCS[4], SOCS112_uv);
		fsCell.extractComponent(SOCS[5], SOCS122_uv);

		for (int i = 0, n = fsFine.mesh.getNv(); i < n; i++) {
			fsFine.mesh.getCoordPerNode(i, xy);
			cellXY[0] = xy[0] * N - (int) (xy[0] * N);
			cellXY[1] = xy[1] * N - (int) (xy[1] * N);
			fsHH.getValue(uv_Homo[0], xy, homoData, eleIdx6, eleVal6);
			uv_0[0][i] = homoData.first;
			uv_0[1][i] = MVO.dot_product(uv_Homo[1], eleIdx6, eleVal6);

			fsCC.getValue(FOCS11_uv[0], cellXY, cellData, eleIdx3, eleVal3);
			val = cellData.first;

			corrector1 = 0.0;
			corrector1 += val * MVO.dot_product(grad[0], eleIdx6, eleVal6); // 11
			corrector1 += MVO.dot_product(FOCS22_uv[0], eleIdx3, eleVal3) * MVO.dot_product(grad[3], eleIdx6, eleVal6); // 22
			corrector1 += MVO.dot_product(FOCS12_uv[0], eleIdx3, eleVal3) * (MVO.dot_product(grad[1], eleIdx6, eleVal6) + MVO.dot_product(grad[2], eleIdx6, eleVal6));
			uv_1[0][i] = uv_0[0][i] + ep * corrector1;

			corrector1 = 0.0;
			corrector1 += MVO.dot_product(FOCS11_uv[1], eleIdx3, eleVal3) * MVO.dot_product(grad[0], eleIdx6, eleVal6); // 11
			corrector1 += MVO.dot_product(FOCS22_uv[1], eleIdx3, eleVal3) * MVO.dot_product(grad[3], eleIdx6, eleVal6); // 22
			corrector1 += MVO.dot_product(FOCS12_uv[1], eleIdx3, eleVal3) * (MVO.dot_product(grad[1], eleIdx6, eleVal6) + MVO.dot_product(grad[2], eleIdx6, eleVal6));
			uv_1[1][i] = uv_0[1][i] + ep * corrector1;

			corrector2 = 0.0;
			corrector2 += MVO.dot_product(SOCS111_uv[0], eleIdx3, eleVal3) * MVO.dot_product(Se[0], eleIdx6, eleVal6); // u1xx
			corrector2 += MVO.dot_product(SOCS121_uv[0], eleIdx3, eleVal3) * MVO.dot_product(Se[2], eleIdx6, eleVal6); // u1xy
			corrector2 += MVO.dot_product(SOCS212_uv[0], eleIdx3, eleVal3) * MVO.dot_product(Se[5], eleIdx6, eleVal6); // u2xy
			corrector2 += MVO.dot_product(SOCS222_uv[0], eleIdx3, eleVal3) * MVO.dot_product(Se[4], eleIdx6, eleVal6); // u2yy
			corrector2 += MVO.dot_product(SOCS112_uv[0], eleIdx3, eleVal3) * (MVO.dot_product(Se[3], eleIdx6, eleVal6) + MVO.dot_product(Se[2], eleIdx6, eleVal6)); // u2xx + u1xy
			corrector2 += MVO.dot_product(SOCS122_uv[0], eleIdx3, eleVal3) * (MVO.dot_product(Se[5], eleIdx6, eleVal6) + MVO.dot_product(Se[1], eleIdx6, eleVal6)); // u2xy + u1yy
			uv_2[0][i] = uv_1[0][i] + ep * ep * corrector2;

			corrector2 = 0.0;
			corrector2 += MVO.dot_product(SOCS111_uv[1], eleIdx3, eleVal3) * MVO.dot_product(Se[0], eleIdx6, eleVal6); // u1xx
			corrector2 += MVO.dot_product(SOCS121_uv[1], eleIdx3, eleVal3) * MVO.dot_product(Se[2], eleIdx6, eleVal6); // u1xy
			corrector2 += MVO.dot_product(SOCS212_uv[1], eleIdx3, eleVal3) * MVO.dot_product(Se[5], eleIdx6, eleVal6); // u2xy
			corrector2 += MVO.dot_product(SOCS222_uv[1], eleIdx3, eleVal3) * MVO.dot_product(Se[4], eleIdx6, eleVal6); // u2yy
			corrector2 += MVO.dot_product(SOCS112_uv[1], eleIdx3, eleVal3) * (MVO.dot_product(Se[3], eleIdx6, eleVal6) + MVO.dot_product(Se[2], eleIdx6, eleVal6)); // u2xx + u1xy
			corrector2 += MVO.dot_product(SOCS122_uv[1], eleIdx3, eleVal3) * (MVO.dot_product(Se[5], eleIdx6, eleVal6) + MVO.dot_product(Se[1], eleIdx6, eleVal6)); // u2xy + u1yy
			uv_2[1][i] = uv_1[1][i] + ep * ep * corrector2;
		}
	}

	FES2T32 fsT3;
	FES2T62 fsT6;
}
